#include "WPILib.h"
class ShooterTest : public IterativeRobot
{

	public:
		bool upPressed, downPressed;
		Talon *motors[7];
		Joystick* stick;
		DriverStationLCD* lcd;
		int cur;
		ShooterTest (void)
		{

		}

		/********************************** Init Routines *************************************/

		void RobotInit (void)
		{
			upPressed = false;
			downPressed = false;
			stick = new Joystick(1);
			for (int i = 1; i <= 7; i++)
				motors[i] = new Talon(i);
			cur = 0;
			lcd = DriverStationLCD::GetInstance();
		}

		void DisabledInit (void)
		{

		}

		void AutonomousInit (void)
		{

		}

		void TeleopInit (void)
		{

		}

		/********************************** Periodic Routines *************************************/

		void DisabledPeriodic (void)
		{

		}

		void AutonomousPeriodic (void)
		{

		}

		void TeleopPeriodic (void)
		{
			if (stick->GetRawButton(5))
			{
				if (!downPressed)
				{
					cur--;
					downPressed = true;
				}
			}
			else downPressed = false;
			
			if (stick->GetRawButton(6))
			{
				if (!upPressed)
				{
					cur++;
					upPressed = true;
				}
			}
			else upPressed = false;
			
			if (cur > 6)
				cur = 0;
			if (cur < 0)
				cur = 6;
			
			motors[cur]->SetSpeed(stick->GetY());
			lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Current Motor: %i", cur + 1);
			lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Joystick Y: %f", stick->GetY());
		}

};

START_ROBOT_CLASS(ShooterTest);
